/**
 * 2024/03/15
 * ssyyw
 * mavproxy_ros1的飞行控制节点，根据当前飞行计划或指令，输出飞行控制命令到mavros
 *
 */

#ifndef FLIGHT_CONTROLLER
#define FLIGHT_CONTROLLER

#include <string>
#include <thread>
#include <atomic>
#include <chrono>
#include <functional>
#include <unordered_map>
#include <ros/ros.h>
#include <cstdlib>
#include <geometry_msgs/PoseStamped.h>
#include <geographic_msgs/GeoPoseStamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/PositionTarget.h>  
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandHome.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/CommandLong.h>
#include <mavros_msgs/WaypointPush.h>
#include <mavros_msgs/WaypointClear.h>
#include <mavros_msgs/WaypointList.h>
#include <mavros_msgs/Waypoint.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <std_msgs/Empty.h>
#include <sensor_msgs/NavSatFix.h>
#include <mavros_msgs/CommandLong.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include "drone_defs.h"

typedef std::function<int()> CommandExecuter;

class FlightController
{
public:
    FlightController();
    ~FlightController(){};

    void InitilizeNode();
    void AvoidanceNavToTarget();

    void CmdCallback(const mavproxy_ros1::FlightCmd::ConstPtr &msg);
    void StateCallback(const mavproxy_ros1::MavproxyState::ConstPtr &msg);
    void PositionCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);

    ros::NodeHandle nh;
    bool obs_enable = false;
    bool is_stop_local = false;

private:
    int HandleCmd();
    int LetDroneSetHome();
    int LetDroneReturn();
    int LetDroneRotate();
    int LetDroneHover();
    int LetDroneFlyToTargetPosition();
    int LetDroneFlyToPosWithObs();
    int SendForOffbPack();
    int LetDroneFlyToPosWithoutObs();
    int LetDroneFlyViaMission();
    int LetDroneLand();
    int LetDroneTakeoff();
    int LetDroneStop();
    int ArmDrone(bool arm);
    int SwitcheModeByName(const std::string &mode_name);
    int SwitcheMode();
    int GetTargetGpsPosition();
    int GetTargetLocalPosition();
    int GetTargetPlannerGoal();
    int ClearMission();
    mavros_msgs::WaypointPush GetTargetMissionWayPoint();

    std::unordered_map<std::string, CommandExecuter> cmd_executers;

    int target_goal_seq = 0;
    bool stop_detected = false;

    ros::Subscriber drone_flight_cmd_sub;
    ros::Subscriber drone_mavproxy_state_sub;
    ros::Subscriber drone_local_positin_sub;


    ros::Publisher drone_target_pose_pub;
    ros::Publisher drone_target_gpose_pub;
    ros::Publisher drone_target_velocity_pub;
    ros::Publisher drone_target_attitude_pub;
    ros::Publisher drone_target_goal_pub;

    ros::ServiceClient drone_arming_client;
    ros::ServiceClient drone_set_home_client;
    ros::ServiceClient drone_mode_client;
    ros::ServiceClient drone_takeoff_cmd_client;
    ros::ServiceClient drone_land_cmd_client;
    ros::ServiceClient drone_push_mission_client;
    ros::ServiceClient drone_clear_mission_client;

    geometry_msgs::QuaternionStamped command_long_srv;
    mavros_msgs::CommandLong return_home_srv;

    geometry_msgs::PoseStamped drone_target_pose;
    geographic_msgs::GeoPoseStamped drone_target_gpose;
    geometry_msgs::TwistStamped drone_target_velocity;
    geometry_msgs::TwistStamped drone_target_attitude;
    geometry_msgs::PoseStamped drone_target_goal;

    mavproxy_ros1::FlightCmd drone_current_cmd;
    mavproxy_ros1::MavproxyState mavproxy_current_state;
};

#endif